#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <iostream>
#include <vector>
#include <math.h>

namespace enc = sensor_msgs::image_encodings;

static const char WINDOW[] = "Image window";

class ImageConverter
{
  ros::NodeHandle nh_;
  image_transport::ImageTransport it_;
  image_transport::Subscriber image_sub;
  image_transport::Publisher image_pub;
  
public:
  ImageConverter()
    : it_(nh_)
  {
    image_pub = it_.advertise("/hough/image_raw", 1);
    image_sub = it_.subscribe("/ardrone/image_raw", 1, &ImageConverter::imageCb, this);

    cv::namedWindow(WINDOW);
  }

  ~ImageConverter()
  {
    cv::destroyWindow(WINDOW);
  }

  void imageCb(const sensor_msgs::ImageConstPtr& msg)
  {
    cv_bridge::CvImagePtr cv_ptr;
    cv::Mat src, dst, color_dst;
    try
    {
      cv_ptr = cv_bridge::toCvCopy(msg, enc::MONO8);
      src = cv_ptr->image;
      cv::Canny( src, dst, 50, 200, 3 );
      cv::cvtColor( dst, color_dst, CV_GRAY2BGR );
      
    }
    catch (cv_bridge::Exception& e)
    {
      ROS_ERROR("cv_bridge exception: %s", e.what());
      return;
    }

    /*std::vector<cv::Vec2f> lines;
    cv::HoughLines(dst, lines, 1, CV_PI/180, 100);
    for( size_t i = 0; i < lines.size(); i++ )
    {
        float rho = lines[i][0];
        float theta = lines[i][1];
        if (theta < (CV_PI/2)+0.2 || theta > (CV_PI/2)-0.2){
          double a = cos(theta), b = sin(theta);
          double x0 = a*rho, y0 = b*rho;
          cv::Point pt1(cvRound(x0 + 1000*(-b)),
                  cvRound(y0 + 1000*(a)));
          cv::Point pt2(cvRound(x0 - 1000*(-b)),
                  cvRound(y0 - 1000*(a)));
          line( color_dst, pt1, pt2, cv::Scalar(0,0,255), 3, 8 );
        }
    }*/
    
    std::vector<cv::Vec4i> lines;
    HoughLinesP( dst, lines, 1, CV_PI/180, 90, 100, 15 );
    for( size_t i = 0; i < lines.size(); i++ )
    {
    	double line_deg = atan2(lines[i][1]-lines[i][3],lines[i][0]-lines[i][2]) * 180 / CV_PI;	//atan2(y-heigh,x-width)
        if(abs(line_deg) < 5.0 || abs(line_deg) > 175){	// angle in degrees
          line( color_dst, cv::Point(lines[i][0], lines[i][1]),
              cv::Point(lines[i][2], lines[i][3]), cv::Scalar(0,0,255), 3, 8 );
          std::cout << "x1: " << lines[i][0] << ", y1: " << lines[i][1]
                    << " | x2: " << lines[i][2] << ", y2: " << lines[i][3];
          std::cout << " | degrees: " << line_deg << std::endl;
    	}
    	//std::cout << "degrees: " << line_deg << std::endl;
    }

    cv::imshow(WINDOW, color_dst);
    cv::waitKey(3);
    
    image_pub.publish(cv_ptr->toImageMsg());
  }
};

int main(int argc, char** argv)
{
  ros::init(argc, argv, "image_converter");
  ImageConverter ic;
  ros::spin();
  return 0;
}
